/*
*  OGF/Graphite: Geometry and Graphics Programming Library + Utilities
*  Copyright (C) 2000-2005 INRIA - Project ALICE
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program; if not, write to the Free Software
*  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*
*  If you modify this software, you should include a notice giving the
*  name of the person performing the modification, the date of modification,
*  and the reason for such modification.
*
*  Contact: Bruno Levy - levy@loria.fr
*
*     Project ALICE
*     LORIA, INRIA Lorraine,
*     Campus Scientifique, BP 239
*     54506 VANDOEUVRE LES NANCY CEDEX
*     FRANCE
*
*  Note that the GNU General Public License does not permit incorporating
*  the Software into proprietary programs.
*
* As an exception to the GPL, Graphite can be linked with the following (non-GPL) libraries:
*     Qt, SuperLU, WildMagic and CGAL
*/


#include "../basic/basic_types.h"
#include "semi_definite_symmetric_eigen.h"



namespace MatrixUtil {

	static const double EPS = 0.00001 ;
	static int MAX_ITER = 100 ;

	void semi_definite_symmetric_eigen(
		const double *mat, int n, double *eigen_vec, double *eigen_val
		) {
			double *a,*v;
			double a_norm,a_normEPS,thr,thr_nn;
			int nb_iter = 0;
			int jj;
			int i,j,k,ij,ik,l,m,lm,mq,lq,ll,mm,imv,im,iq,ilv,il,nn;
			int *index;
			double a_ij,a_lm,a_ll,a_mm,a_im,a_il;
			double a_lm_2;
			double v_ilv,v_imv;
			double x;
			double sinx,sinx_2,cosx,cosx_2,sincos;
			double delta;

			// Number of entries in mat

			nn = (n*(n+1))/2;

			// Step 1: Copy mat to a

			a = new double[nn];

			for( ij=0; ij<nn; ij++ ) {
				a[ij] = mat[ij];
			}

			// Ugly Fortran-porting trick: indices for a are between 1 and n
			a--;

			// Step 2 : Init diagonalization matrix as the unit matrix
			v = new double[n*n];

			ij = 0;
			for( i=0; i<n; i++ ) {
				for( j=0; j<n; j++ ) {
					if( i==j ) {
						v[ij++] = 1.0;
					} else {
						v[ij++] = 0.0;
					}
				}
			}

			// Ugly Fortran-porting trick: indices for v are between 1 and n
			v--;

			// Step 3 : compute the weight of the non diagonal terms 
			ij     = 1  ;
			a_norm = 0.0;
			for( i=1; i<=n; i++ ) {
				for( j=1; j<=i; j++ ) {
					if( i!=j ) {
						a_ij    = a[ij];
						a_norm += a_ij*a_ij;
					}
					ij++;
				}
			}

			if( a_norm != 0.0 ) {

				a_normEPS = a_norm*EPS;
				thr       = a_norm    ;

				// Step 4 : rotations
				while( thr > a_normEPS   &&   nb_iter < MAX_ITER ) {

					nb_iter++;
					thr_nn = thr / nn;

					for( l=1  ; l< n; l++ ) {
						for( m=l+1; m<=n; m++ ) {

							// compute sinx and cosx 

							lq   = (l*l-l)/2;
							mq   = (m*m-m)/2;

							lm     = l+mq;
							a_lm   = a[lm];
							a_lm_2 = a_lm*a_lm;

							if( a_lm_2 < thr_nn ) {
								continue ;
							}

							ll   = l+lq;
							mm   = m+mq;
							a_ll = a[ll];
							a_mm = a[mm];

							delta = a_ll - a_mm;

							if( delta == 0.0 ) {
								x = - M_PI/4 ; 
							} else {
								x = - std::atan( (a_lm+a_lm) / delta ) / 2.0 ;
							}

							sinx    = std::sin(x)   ;
							cosx    = std::cos(x)   ;
							sinx_2  = sinx*sinx;
							cosx_2  = cosx*cosx;
							sincos  = sinx*cosx;

							// rotate L and M columns 

							ilv = n*(l-1);
							imv = n*(m-1);

							for( i=1; i<=n;i++ ) {
								if( (i!=l) && (i!=m) ) {
									iq = (i*i-i)/2;

									if( i<m )  { 
										im = i + mq; 
									} else {
										im = m + iq;
									}
									a_im = a[im];

									if( i<l ) {
										il = i + lq; 
									} else {
										il = l + iq;
									}
									a_il = a[il];

									a[il] =  a_il*cosx - a_im*sinx;
									a[im] =  a_il*sinx + a_im*cosx;
								}

								ilv++;
								imv++;

								v_ilv = v[ilv];
								v_imv = v[imv];

								v[ilv] = cosx*v_ilv - sinx*v_imv;
								v[imv] = sinx*v_ilv + cosx*v_imv;
							} 

							x = a_lm*sincos; x+=x;

							a[ll] =  a_ll*cosx_2 + a_mm*sinx_2 - x;
							a[mm] =  a_ll*sinx_2 + a_mm*cosx_2 + x;
							a[lm] =  0.0;

							thr = std::fabs( thr - a_lm_2 );
						}
					}
				}         
			}

			// Step 5: index conversion and copy eigen values 

			// back from Fortran to C++
			a++;

			for( i=0; i<n; i++ ) {
				k = i + (i*(i+1))/2;
				eigen_val[i] = a[k];
			}

			delete[] a;

			// Step 6: sort the eigen values and eigen vectors 

			index = new int[n];
			for( i=0; i<n; i++ ) {
				index[i] = i;
			}

			for( i=0; i<(n-1); i++ ) {
				x = eigen_val[i];
				k = i;

				for( j=i+1; j<n; j++ ) {
					if( x < eigen_val[j] ) {
						k = j;
						x = eigen_val[j];
					}
				}

				eigen_val[k] = eigen_val[i];
				eigen_val[i] = x;

				jj       = index[k];
				index[k] = index[i];
				index[i] = jj;
			}


			// Step 7: save the eigen vectors 

			v++; // back from Fortran to to C++

			ij = 0;
			for( k=0; k<n; k++ ) {
				ik = index[k]*n;
				for( i=0; i<n; i++ ) {
					eigen_vec[ij++] = v[ik++];
				}
			}

			delete[] v    ;
			delete[] index;
			return;
	}

	//_________________________________________________________
}
